/*!
  @file camera_driver.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include <ros/ros.h>
#include <nodelet/nodelet.h>

#include <cstdlib>

#include <curl/curl.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/image_encodings.h>

#include "axis_driver/camera_driver.h"

using namespace std;

namespace axis_netcam
{

void CameraDriver::init()
{
  /* generate url */
  url_ = (boost::format("http://%s/axis-cgi/jpg/image.cgi?resolution=%s") %camera_ipaddr_ %resolution_).str();
  //ROS_INFO_STREAM("send request to " << url_.str());

  /* prepare buffer */
  chunk_.memory = (char*)malloc(1);
  chunk_.size = 0;

  /* curl init */
  curl_global_init(CURL_GLOBAL_ALL);
  curl_ = curl_easy_init();
  if (curl_)
  {
    curl_easy_setopt(curl_, CURLOPT_URL, url_.c_str());
    curl_easy_setopt(curl_, CURLOPT_FOLLOWLOCATION, 1L);
    curl_easy_setopt(curl_, CURLOPT_WRITEFUNCTION, &CameraDriver::htmlCB);
    curl_easy_setopt(curl_, CURLOPT_WRITEDATA, (void *)&chunk_);
  }
}

void CameraDriver::run()
{
  ros::Rate rate(frame_rate_);
  while (nh_.ok())
  {
    poll();

    ros::spinOnce();
    rate.sleep();
  }
}

int CameraDriver::poll()
{
  CURLcode res;
  res = curl_easy_perform(curl_);
  if (res != CURLE_OK)
  {
    ROS_WARN("curl_easy_perform() failed: %s", curl_easy_strerror(res));
    return -1;
  }
  else
  {
    /* decode and publish */
    cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);
    cv::Mat buf(1, chunk_.size, CV_8UC3, chunk_.memory);
    cv_ptr->image = cv::imdecode(buf, 1);
    flip(cv_ptr->image, cv_ptr->image, -1);
    cv_ptr->header.stamp    = ros::Time::now();
    cv_ptr->header.frame_id = nh_.resolveName("camera_frame");
    cv_ptr->encoding        = sensor_msgs::image_encodings::BGR8;

    sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
    ci->header.stamp    = cv_ptr->header.stamp;
    ci->header.frame_id = cv_ptr->header.frame_id;
    ci->width  = cv_ptr->image.cols;
    ci->height = cv_ptr->image.rows;

    img_pub_.publish(cv_ptr->toImageMsg(), ci);

    /* reset buffer */
    chunk_.size = 0;
  }
  return 0;
}
  
size_t CameraDriver::htmlCB(void *contents, size_t size, size_t nmemb, void *userp)
{  
  size_t realsize = size * nmemb;
  struct MemoryStruct *mem = (struct MemoryStruct *)userp;

  mem->memory = (char*)realloc(mem->memory, mem->size + realsize + 1);
  if(mem->memory == NULL) {
    /* out of memory! */ 
    ROS_ERROR("not enough memory (realloc returned NULL)");
    return 0;
  }

  memcpy(&(mem->memory[mem->size]), contents, realsize);
  mem->size += realsize;
  mem->memory[mem->size] = 0;

  return realsize;
}

}  // namespace axis_netcam
